package novemberBot.communication;

import novemberBot.utils.ByteArray;
import lejos.nxt.comm.NXTConnection;

/**Thread which monitors an NXTConnection object and passes along data 
 * to select classes. The first byte of incoming data should indicate the amount of bytes of actual data
 * The second byte should serve to indicate where the ConnectionMonitor should send the received data.
 * The third to final bytes represent the data itself.
 * */
public class ConnectionManager extends Thread {
	/**special reserved ID for ping*/
	public static final byte PING = 127;
	private boolean awaitingPing=false;
	/**Connection object*/
	private NXTConnection nc;
	/**status - 0:waiting/doing nothing. 1: sending, 2: receiving*/
	private byte status;
	/**Lock for synchronizing status byte*/
	private Object statusLock = new Object();
	/**data waiting to be sent*/
	private byte[] pendingData;
	/**destination for pendingData*/
	private byte pdDest;
	/**Lock for synchronizing data and data destination*/
	private Object dataLock = new Object();
	/**array with all the RS485 responders*/
	private RS485Responder[] responders = new RS485Responder[0];
	
	/**Basic constructor
	 * @param nc - connection
	 * @param start - immediate start of monitoring*/
	public ConnectionManager(NXTConnection nc, boolean start){
		this.nc=nc;
		nc.setIOMode(NXTConnection.PACKET);
		setStatus(0);
		if(start)this.start();
	}
	
	public long ping() {return ping(1);}
	/**pings the other brick and waits for the return ping
	 * @param length - amount of bytes to send as bogus data in ping
	 * @return time taken for round trip in ms, -1 for 0 length*/
	public long ping(int length){
		if(length==0) return -1;
		byte[] data = new byte[length];
		awaitingPing=true;
		long sendTime=System.currentTimeMillis();
		setPending(data, PING);
		requestSend();
		while(awaitingPing==true)Thread.yield();
		return System.currentTimeMillis()-sendTime;
	}
	
	
	/**waits for data to be received and passes it along to the destination.
	 * when not reading data can be used to send back*/
	public void run(){
		while(true){
			while(nc.available(1)<3 && getStatus()!=1)Thread.yield();
			if(getStatus()==1)sendPending();
			else{
				setStatus(2);
				byte [] sizeAndDest=new byte[2];
				nc.readPacket(sizeAndDest, 2);
				while(nc.available(1)<sizeAndDest[0]) Thread.yield();
				byte data[] = new byte[sizeAndDest[0]];
				nc.readPacket(data, sizeAndDest[0]);
				passAlongData(data, sizeAndDest[1]);
				if(getpdDest()!=PING)setStatus(0);
				//so that requestTo send can latch
				try {
					Thread.sleep(10);
				} catch (InterruptedException e) {}
			}
		}
	}
	/**helper for run
	 * sends the data do the right RS485responder, also responds to pings*/
	private void passAlongData(byte[] data, byte destination){
		if(destination==PING){
			if(awaitingPing) awaitingPing=false;
			else {
				setPending(data, PING);
				requestSend();
			}
		}
		if(responders==null) return;
		for(int i=0; i<responders.length; i++){
			if(responders[i].getRSId()==destination)
				
				responders[i].newRS485Data(data);
		}
	}
	
	/**method for sending an int to a specific destination on the other brick.
	 * returns once data is about to be sent
	 * @param i - the int to be passed
	 * @param destination - byte representation of destination*/
	public void passInt(int i, byte destination){
		setPending(ByteArray.fromInt(i), destination);
		requestSend();	
	}
	/**=NOT WORKING=
	 * method for sending a double to a specific destination on the other brick.
	 * returns once data is about to be sent 
	 * @param d - the double to be passed
	 * @param destination - byte representation of destination*/
	public void passDouble(double d, byte destination){
		setPending(ByteArray.fromDouble(d), destination);
		requestSend();
	}
	/**method for sending a String to a specific destination on the other brick.
	 * returns once data is about to be sent
	 * @param s - the String to be passed
	 * @param destination - byte representation of destination*/
	public void passString(String s, byte destination){
		setPending(ByteArray.fromString(s), destination);
		requestSend();
	}
	/**Method for sending raw data*/
	public void passRaw(byte[] rawData, byte destination){
		setPending(rawData, destination);
		requestSend();
	}

	
	
	
	/**holds on to Thread till the ConnectionManager is ready to send data*/
	private void requestSend(){
		while(getStatus()!=0 && getpdDest()!=PING)Thread.yield();
		setStatus(1);
	}
	
	/**called by run sends pending data to other brick*/
	private void sendPending(){
		//Debugger.breakPoint("about to send");
		byte[] data=getPendingData();
		byte destination = getpdDest();
		byte[] dataAndInfo=new byte[data.length+2];
		dataAndInfo[0]=(byte) data.length;
		dataAndInfo[1]=destination;
		for(int i=0; i<data.length; i++){
			dataAndInfo[i+2]=data[i];
		}
		nc.sendPacket(dataAndInfo, dataAndInfo.length);
		//Debugger.breakPoint("sent");
		setStatus(0);
	}

	
	//accessors
	public byte getStatus(){
		synchronized(statusLock){return status;}
	}
	
	private byte[] getPendingData(){
		synchronized(dataLock){
			return pendingData;
		}
	}
	
	private byte getpdDest(){
		synchronized(dataLock){
			return pdDest;
		}
	}
	//mutators
//	public void setResponders(RS485Responder[] responders){
	//	this.responders=responders;
//	}
	
	public void addResponder(RS485Responder responder){
		RS485Responder[] resp = new RS485Responder[responders.length+1];
		for(int i=0; i<responders.length; i++)
			resp[i]=responders[i];
		resp[responders.length]=responder;
		responders=resp;
	}
	
	public void setStatus(int status){
		synchronized(statusLock){
			this.status=(byte)status;
		}
	}
	
	
	private void setPending(byte[] data, byte destination){
		synchronized(dataLock){
			pendingData=data.clone();
			pdDest=destination;
		}
	}
}
